#include "ros/ros.h"
#include "saint_nr_9/IRdata.h"
#include <list>
#include "importantdefines.h"
#include <std_msgs/Int32.h>
#include <cmath>

#define LENGTH_DETECT_HOLE 3
#define DISTANCE_LEAD_TO_HOLE 10
#define BLUE 0 
#define YELLOW 1
#define RED 2
#define WHITE 3
#define BLACK 4
#define ORANGE 5
#define GREEN 6

#define HIGHEST_CORRECT_READING_SHORT 27


std::list<double> histIRReadings[7];

bool detectHoleOnWhite = false;
bool detectHole = false;
bool robotAtAWall = true;
bool detectWallOnWhite = false;
ros::Publisher pub_Wall;

bool holeInReading(std::list<double> histReading)
{
	//ROS_INFO("Check for hole in reading");
	//ROS_INFO("%f", histReading.back() - histReading.front());
	return ((histReading.back() - histReading.front()) > DISTANCE_LEAD_TO_HOLE && histReading.back() < HIGHEST_CORRECT_READING_SHORT);
	
}

bool startingWallInReading(std::list<double> histReading)
{
	//ROS_INFO("Check for wall");
	ROS_INFO("%f", histReading.front() - histReading.back());
	return ((histReading.front() - histReading.back()) > DISTANCE_LEAD_TO_HOLE && histReading.front() < HIGHEST_CORRECT_READING_SHORT);
	
}

void IRCallback(const saint_nr_9::IRdata::ConstPtr& data) {
	histIRReadings[YELLOW].push_back(data->yellow);
  	histIRReadings[RED].push_back(data->red);
  	histIRReadings[WHITE].push_back(data->white);
  	histIRReadings[BLACK].push_back(data->black);
  	histIRReadings[BLUE].push_back
  	
  	(data->blue);
  	histIRReadings[ORANGE].push_back(data->orange);
  	histIRReadings[GREEN].push_back(data->green);
  	
  	if (histIRReadings[YELLOW].size() > LENGTH_DETECT_HOLE)
  	{
  		histIRReadings[YELLOW].pop_front();
  		histIRReadings[RED].pop_front();
  		histIRReadings[WHITE].pop_front();
  		histIRReadings[BLACK].pop_front();
  		histIRReadings[BLUE].pop_front();
  		histIRReadings[ORANGE].pop_front();
  		histIRReadings[GREEN].pop_front();
  	}
  	
  	if (histIRReadings[YELLOW].size() >= LENGTH_DETECT_HOLE)
  	{ 
  		//DETECT WHATEVER IS AT A WALL
  		if (detectHole)
  		{
  		
  		 //IF I detected a hole somewhere before I have to wait to find the wall again before I do something else
  			if (!detectWallOnWhite && startingWallInReading(histIRReadings[WHITE]))
  			{
  				detectWallOnWhite = true;
  			}
  			else if(detectWallOnWhite && startingWallInReading(histIRReadings[ORANGE]))
  			{
  				detectWallOnWhite = false;
  				robotAtAWall = true;
  				detectHole = false;
  				std_msgs::Int32 p;
				p.data = WALLAGAIN;
				for (int i = 0; i < 5; i++)
					pub_Wall.publish(p);
				ROS_INFO("AT the wall again :) ");
  			}
  		}	
  		
  		if (detectHoleOnWhite)
  		{
  			if (holeInReading(histIRReadings[ORANGE]) && robotAtAWall)
  			{
  				ROS_INFO("I found a hole in the shiny wall");
  				std_msgs::Int32 p;
  				p.data = HOLE;
  				for (int i = 0; i < 5; i++)
  					pub_Wall.publish(p);
  				detectHoleOnWhite = false;
  				detectHole = true;
  				robotAtAWall = false;
  				//return;
  			}		
  		}
  		else if (holeInReading(histIRReadings[WHITE]))
  		{
  			//I can start to detect a hole even if I dont found the wall again before.
  			ROS_INFO("I try to detect a hole, first sensor says so oO");
  			std_msgs::Int32 p;
			p.data = HOLE_DETECTION;
			for (int i = 0; i < 5; i++)
				pub_Wall.publish(p);
  			detectHoleOnWhite = true;
  		}
  		//Here goes to far to close detection
  		else if (histIRReadings[WHITE].front() < MIN_REACT_TO_SIDE)
  		{
  			std_msgs::Int32 p;
			p.data = TO_CLOSE;
			pub_Wall.publish(p);
  		}
  		else if (histIRReadings[WHITE].front() > MAX_REACT_TO_SIDE)
  		{
  			std_msgs::Int32 p;
			p.data = TO_FAR_AWAY;
			pub_Wall.publish(p);
  		}
  		 8 
  	}
  	
}



int main(int argc, char**argv) {
  ros::init(argc, argv, "StateDeterminator");
  sleep(2);
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("ir_info", 1000, IRCallback);
  pub_Wall = n.advertise<std_msgs::Int32>("state_info", 1);
  ROS_INFO("StateDeterminator running");
  ros::spin();
}
